package nape.shape;
import nape.shape.Circle;
import nape.shape.Shape;
import nape.geom.AABB;
import nape.geom.Axis;
import nape.geom.Mat23;
import nape.geom.Vec2;
import nape.geom.VecMath;
import nape.util.FastMath;
import nape.util.MixPool;
import nape.util.Names;
import nape.util.NpAlgorithm;
import nape.util.NpList;
import nape.phys.Material;
import nape.Config;
import nape.Const;
import nape.util.NpNode_ZNContact;
import nape.util.NpAllocList_ZNContact;
import nape.util.NpNode_ZNV2;
import nape.util.NpList_ZNV2;
import nape.util.NpNode_Vec2__Dynamic;
import nape.util.NpList_Vec2__Dynamic;
import nape.util.NpNode_ZNShp;
import nape.util.NpList_ZNShp;










class Polygon extends Shape {
    public var znp_inner:ZNPoly;
    
    public function new(vertices:Array<Vec2>, ?offset:Vec2, ?material:Material) {
        if(material==null) material = Config.DEFAULT_MATERIAL;
        znp_inner = new ZNPoly(vertices,offset,material.znp_inner);
        znp_inner.outer = this;
        
        znp_inner.outer_shape = this;
        znp_inner_shape = znp_inner;
    }
    
    public var localVertices(get_localVertices,never):MutableVec2List;
    public inline function get_localVertices() {
        return new MutableVec2List(znp_inner.localVerts,znp_inner.mod_localVerts,znp_inner.validate_localVerts);
    }
    
    public var axi(get_axi,never):ImmutableAxisList;
    public inline function get_axi() {
        return new ImmutableAxisList(znp_inner.axi,znp_inner.validate_axi);
    }
}

class ZNPoly extends ZNShp {
    public var outer:Polygon;
    
    public var localVerts: ZNV2;
    public var worldVerts: ZNV2;
    public var axi       : ZNAxis;
    
    public function mod_localVerts(?_:Vec2) invalidate_localVerts()
    
    
    public function new(vertices:Array<Vec2>,offset:Vec2,material:ZNMat) {
        super(Shape.POLYGON,material);
        polygon = this;

               var offx:Float;  var offy:Float;
        if(offset==null)  { offx = 0; offy = 0; };
        else     { offx = offset.znp_inner.x; offy = offset.znp_inner.y; };
        
        localVerts = new  ZNV2();
        worldVerts = new  ZNV2();
        axi        = new  ZNAxis();

        var p0 = null;
        var count = vertices.length;
        for(i in 0...count) {
            var u = vertices[i].znp_inner.clone();
            var v = vertices[(i+1)%count].znp_inner;
            
            var a = new ZNAxis();
                   var nx:Float;  var ny:Float;  { nx = u.x-v.x; ny = u.y-v.y; };
            a.edgeLength =  Math.sqrt(    (nx*nx + ny*ny));
             {
     {
    var d =     (nx*nx + ny*ny);
    var imag = if(d<Const.EPSILON) 0 else 1.0/Math.sqrt(d);
     { var t = (imag); nx *= t; ny *= t; };
};
     {
    var t = nx;
    nx = -ny;
    ny = t;
};
};
              { var t = (1.0); u.x += offx*t; u.y += offy*t; };
            
                { a.localNormalx = nx; a.localNormaly = ny; };
                { a.worldNormalx = nx; a.worldNormaly = ny; };
            a.projection =  (nx*u.x + ny*u.y);
            
            if(i!=0) {
                localVerts.add(u);
                worldVerts.add(u.clone());
            }else p0 = u;
            
            axi.add(a);
        }
        localVerts.add(p0);
        worldVerts.add(p0.clone());
    }
    
    
    
    
    public inline function __validate_area_inertia() {
        var ls = 0.0;
        area = 0.0;
        inertia = 0.0;
         { localCOMx = 0; localCOMy = 0; };
            
         {
    var npite = localVerts.begin();
    var u = npite.elem();
    npite = npite.next;
    var v = npite.elem();
    npite = npite.next;
    
    while(npite!=null) {
        var w = npite.elem();
        {
            {
            area += v.x*(w.y-u.y);
            var dot =  (v.y*u.x - v.x*u.y);
                
            localCOMx += (u.x+v.x)*dot;
            localCOMy += (u.y+v.y)*dot;
            var lsq =     (u.x*u.x + u.y*u.y) +  (u.x*v.x + u.y*v.y) +     (v.x*v.x + v.y*v.y);
                
            inertia += dot*lsq;
            ls += dot;
        };
        }
        u = v; v = w;
        npite = npite.next;
    }
    
    var w = (npite = localVerts.begin()).elem();
    {
        {
            area += v.x*(w.y-u.y);
            var dot =  (v.y*u.x - v.x*u.y);
                
            localCOMx += (u.x+v.x)*dot;
            localCOMy += (u.y+v.y)*dot;
            var lsq =     (u.x*u.x + u.y*u.y) +  (u.x*v.x + u.y*v.y) +     (v.x*v.x + v.y*v.y);
                
            inertia += dot*lsq;
            ls += dot;
        };
    }
    u = v; v = w;
    w = npite.next.elem();
    {
        {
            area += v.x*(w.y-u.y);
            var dot =  (v.y*u.x - v.x*u.y);
                
            localCOMx += (u.x+v.x)*dot;
            localCOMy += (u.y+v.y)*dot;
            var lsq =     (u.x*u.x + u.y*u.y) +  (u.x*v.x + u.y*v.y) +     (v.x*v.x + v.y*v.y);
                
            inertia += dot*lsq;
            ls += dot;
        };
    }
};
            
        area *= 0.5;
        inertia /= 6*ls;
          { var t = (1.0/(6*area)); localCOMx *= t; localCOMy *= t; };
            
        if(area<0) {
            area = -area;
            invalidate_axi();
            localVerts.reverse();
             _inv_localVerts = false;
        }
        
         _inv_localCOM =  _inv_area_inertia = false;
        if(wrap_localCOM!=null)
                { wrap_localCOM.znp_inner.x = localCOMx; wrap_localCOM.znp_inner.y = localCOMy; };   
    }
    
    public inline function __validate_angDrag() {
        var accum = 0.0;
        var num = 0;
        
         {
    var npite = localVerts.begin();
    var u = npite.elem();
    npite = npite.next;
    
    while(npite!=null) {
        var v = npite.elem();
        {
            {
                   var qx:Float;  var qy:Float;  { qx = v.x-u.x; qy = v.y-u.y; };
                 var nx:Float = -qy; var ny:Float = qx;
            
            var qls =     (qx*qx + qy*qy);
            var ql = Math.sqrt(qls);
              { var t = (1.0/(ql)); nx *= t; ny *= t; };
            var numc = Std.int(ql/Config.ANGDRAGCOEF_STEPSIZE+1);   { var t = (1.0/(numc)); qx *= t; qy *= t; };
            for(i in 0...numc+1) {
                       var px:Float;  var py:Float;     { px = u.x; py = u.y; };  { var t = (i); px += qx*t; py += qy*t; };
                     var fx:Float = py; var fy:Float = -px;
                
                var x =  (nx*fx + ny*fy);
                if(x>0) x *= Config.ANGDRAGCOEF_VACUUM;
                
                var y =  (ny*fx - nx*fy);
                y *= material.dynamicFriction*Config.ANGDRAGCOEF_FRICTION;
                
                     var prx:Float = nx*x-ny*y; var pry:Float = ny*x+nx*y;
                accum +=  (pry*px - prx*py);
            }
            num += numc+1;
        };
        }
        u = v;
        npite = npite.next;
    }
    var v = localVerts.front();
    {
        {
                   var qx:Float;  var qy:Float;  { qx = v.x-u.x; qy = v.y-u.y; };
                 var nx:Float = -qy; var ny:Float = qx;
            
            var qls =     (qx*qx + qy*qy);
            var ql = Math.sqrt(qls);
              { var t = (1.0/(ql)); nx *= t; ny *= t; };
            var numc = Std.int(ql/Config.ANGDRAGCOEF_STEPSIZE+1);   { var t = (1.0/(numc)); qx *= t; qy *= t; };
            for(i in 0...numc+1) {
                       var px:Float;  var py:Float;     { px = u.x; py = u.y; };  { var t = (i); px += qx*t; py += qy*t; };
                     var fx:Float = py; var fy:Float = -px;
                
                var x =  (nx*fx + ny*fy);
                if(x>0) x *= Config.ANGDRAGCOEF_VACUUM;
                
                var y =  (ny*fx - nx*fy);
                y *= material.dynamicFriction*Config.ANGDRAGCOEF_FRICTION;
                
                     var prx:Float = nx*x-ny*y; var pry:Float = ny*x+nx*y;
                accum +=  (pry*px - prx*py);
            }
            num += numc+1;
        };
    }
};
        
        if(accum<0) accum = -accum;
        angDrag = accum/(num*area);
    }
    
    
    
    public inline function __validate_localCOM() {
        __validate_area_inertia();
    }
    
    

    public inline function __translate(x:Float,y:Float) {
        var ai = axi.begin();
          {
    var npiterator = localVerts.begin();
    while(npiterator != null) {
        var p = npiterator.elem();
        {
            
            
            {
              { var t = (1.0); p.x += x*t; p.y += y*t; };
            var axis = ai.elem(); ai = ai.next;
            axis.projection =  (axis.localNormalx*p.x + axis.localNormaly*p.y);
        };
        }
        npiterator = npiterator.next;
    }
};
                
        invalidate_localVerts();
    }
    public inline function __scale(scaleX:Float,scaleY:Float) {
          {
    var npiterator = localVerts.begin();
    while(npiterator != null) {
        var p = npiterator.elem();
        {
            
            
            {
            p.x *= scaleX;
            p.y *= scaleY;
        };
        }
        npiterator = npiterator.next;
    }
};
        
        invalidate_localVerts();
    }
    public inline function __rotate(rx:Float,ry:Float) {
          {
    var npiterator = localVerts.begin();
    while(npiterator != null) {
        var p = npiterator.elem();
        {
            
            
            {
             {
    var t =  (p.y*rx - p.x*ry);
    p.y =  (p.x*rx + p.y*ry);
    p.x = t;
};
        };
        }
        npiterator = npiterator.next;
    }
};
        
        invalidate_localVerts();
    }
    public inline function __transform(mat:Mat23) {
          {
    var npiterator = localVerts.begin();
    while(npiterator != null) {
        var p = npiterator.elem();
        {
            
            
            {
             {
    var t = mat.a*p.x + mat.b*p.y;
    p.y   = mat.c*p.x + mat.d*p.y;
    p.x   = t;
};
              { var t = (1.0); p.x += mat.tx*t; p.y += mat.ty*t; };
        };
        }
        npiterator = npiterator.next;
    }
};
        
        invalidate_localVerts();
    }
    
    
    
    public var  _inv_localVerts:Bool;
    public inline function invalidate_localVerts() {
        invalidate_localCOM();
        invalidate_axi();
         _inv_localVerts = true;
    }
    
    public var  _inv_axi:Bool;
    public inline function invalidate_axi() {
         _inv_axi = true;
    }
    
    
    
    public inline function validate_localVerts() {
        if( _inv_localVerts) {
             _inv_localVerts = false;
            
            validate_area_inertia();
        }
    }
    
    public inline function validate_axi() {
        if( _inv_axi) {
             _inv_axi = false;
            var pre = null;
            var ai = axi.begin();
             {
    var npite = localVerts.begin();
    var u = npite.elem();
    npite = npite.next;
    
    while(npite!=null) {
        var v = npite.elem();
        {
            {
                if(ai==null) {
                    axi.insert(pre,new ZNAxis());
                    ai = pre.next;
                }
                var axis = ai.elem();
                pre = ai;
                ai = ai.next;
                
                       var nx:Float;  var ny:Float;  { nx = v.x-u.x; ny = v.y-u.y; };
                axis.edgeLength =  Math.sqrt(    (nx*nx + ny*ny));
                 {
     {
    var d =     (nx*nx + ny*ny);
    var imag = if(d<Const.EPSILON) 0 else 1.0/Math.sqrt(d);
     { var t = (imag); nx *= t; ny *= t; };
};
     {
    var t = nx;
    nx = -ny;
    ny = t;
};
};
                    { axis.localNormalx = nx; axis.localNormaly = ny; };
                axis.projection =  (axis.localNormalx*u.x + axis.localNormaly*u.y);
                    { axis.worldNormalx = nx; axis.worldNormaly = ny; };
                
                
                if(axis.wrap_localNormal!=null)
                        { axis.wrap_localNormal.znp_inner.x = axis.localNormalx; axis.wrap_localNormal.znp_inner.y = axis.localNormaly; };
            };
        }
        u = v;
        npite = npite.next;
    }
    var v = localVerts.front();
    {
        {
                if(ai==null) {
                    axi.insert(pre,new ZNAxis());
                    ai = pre.next;
                }
                var axis = ai.elem();
                pre = ai;
                ai = ai.next;
                
                       var nx:Float;  var ny:Float;  { nx = v.x-u.x; ny = v.y-u.y; };
                axis.edgeLength =  Math.sqrt(    (nx*nx + ny*ny));
                 {
     {
    var d =     (nx*nx + ny*ny);
    var imag = if(d<Const.EPSILON) 0 else 1.0/Math.sqrt(d);
     { var t = (imag); nx *= t; ny *= t; };
};
     {
    var t = nx;
    nx = -ny;
    ny = t;
};
};
                    { axis.localNormalx = nx; axis.localNormaly = ny; };
                axis.projection =  (axis.localNormalx*u.x + axis.localNormaly*u.y);
                    { axis.worldNormalx = nx; axis.worldNormaly = ny; };
                
                
                if(axis.wrap_localNormal!=null)
                        { axis.wrap_localNormal.znp_inner.x = axis.localNormalx; axis.wrap_localNormal.znp_inner.y = axis.localNormaly; };
            };
    }
};
            
            while(pre.next!=null) axi.erase(pre,pre.next);
        }
    }
}
